
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       rc_protocol.c
  * @author     baiyang
  * @date       2022-6-11
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "rc_protocol.h"
#include "rcprotocol_sbus.h"
#include "rcprotocol_ppmsum.h"

#include <assert.h>

#include <rtconfig.h>
#include <rtdevice.h>

#include <rc_channel/rc.h>
#include <common/time/gp_time.h>
#include <rc_channel/rc_channel.h>
#include <serial_manager/gp_serial.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void rcprotocol_check_added_uart(void);
static bool rcprotocol_protocol_enabled(enum rcprotocol_t protocol);
static void serialconfig_apply_to_uart(const struct SerialConfig* serialconfig, rt_device_t uart);
/*----------------------------------variable----------------------------------*/
struct rc_protocol rcprotocol;

static const struct SerialConfig serial_configs[] = {
    // BAUD PRTY STOP INVERT-RX
    // inverted and uninverted 115200 8N1:
    { 115200,  0,   1, false  },
    { 115200,  0,   1, true },
    // SBUS settings, even parity, 2 stop bits:
    { 100000,  2,   2, true },
#if AP_RCPROTOCOL_FASTSBUS_ENABLED
    // FastSBUS:
    { 200000,  2,   2, true },
#endif
    // CrossFire:
    { 416666,  0,   1, false },
};

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
_Static_assert(ARRAY_SIZE(serial_configs) > 1, "must have at least one serial config");
#else
static_assert(ARRAY_SIZE(serial_configs) > 1, "must have at least one serial config");
#endif
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void rcprotocol_ctor()
{
    rcprotocol._detected_protocol = RCP_NONE;
}

void rcprotocol_destructor()
{
    for (uint8_t i = 0; i < RCP_NONE; i++) {
        if (rcprotocol.backend[i] != NULL) {
            rcprotocol_backend_destructor(rcprotocol.backend[i]);
            rcprotocol.backend[i] = NULL;
        }
    }
}

void rcprotocol_init()
{
    rcprotocol.backend[RCP_PPM] = rcprotocol_ppmsum_probe();
    rcprotocol.backend[RCP_SBUS] = rcprotocol_sbus_probe(true, 100000);
#if AP_RCPROTOCOL_FASTSBUS_ENABLED
    rcprotocol.backend[RCP_FASTSBUS] = rcprotocol_sbus_probe(true, 200000);
#endif
    rcprotocol.backend[RCP_SBUS_NI] = rcprotocol_sbus_probe(false, 100000);
}

bool rcprotocol_should_search(uint32_t now_ms)
{
    if (rcprotocol._detected_protocol != RCP_NONE && !rcs_multiple_receiver_support()) {
        return false;
    }

    return (now_ms - rcprotocol._last_input_ms >= 200);
}

void rcprotocol_process_pulse(uint32_t width_s0, uint32_t width_s1)
{
    uint32_t now = time_millis();
    bool searching = rcprotocol_should_search(now);

    rcprotocol.rc_protocols_mask = rcs_enabled_protocols();

    if (rcprotocol._detected_protocol != RCP_NONE &&
        !rcprotocol_protocol_enabled(rcprotocol._detected_protocol)) {
        rcprotocol._detected_protocol = RCP_NONE;
    }
    
    if (rcprotocol._detected_protocol != RCP_NONE && rcprotocol._detected_with_bytes && !searching) {
        // we're using byte inputs, discard pulses
        return;
    }
    // first try current protocol
    if (rcprotocol._detected_protocol != RCP_NONE && !searching) {
        rcprotocol_backend_process_pulse(rcprotocol.backend[rcprotocol._detected_protocol], width_s0, width_s1);
        if (rcprotocol_backend_new_input(rcprotocol.backend[rcprotocol._detected_protocol])) {
            rcprotocol._new_input = true;
            rcprotocol._last_input_ms = now;
        }
        return;
    }

    // otherwise scan all protocols
    for (uint8_t i = 0; i < RCP_NONE; i++) {
        if (rcprotocol._disabled_for_pulses & (1U << i)) {
            // this protocol is disabled for pulse input
            continue;
        }
        if (rcprotocol.backend[i] != NULL) {
            if (!rcprotocol_protocol_enabled((enum rcprotocol_t)i)) {
                continue;
            }
            const uint32_t frame_count = rcprotocol_backend_get_rc_frame_count(rcprotocol.backend[i]);
            const uint32_t input_count = rcprotocol_backend_get_rc_input_count(rcprotocol.backend[i]);
            rcprotocol_backend_process_pulse(rcprotocol.backend[i], width_s0, width_s1);
            const uint32_t frame_count2 = rcprotocol_backend_get_rc_frame_count(rcprotocol.backend[i]);
            if (frame_count2 > frame_count) {
                if (rcprotocol_requires_3_frames((enum rcprotocol_t)i) && frame_count2 < 3) {
                    continue;
                }
                rcprotocol._new_input = (input_count != rcprotocol_backend_get_rc_input_count(rcprotocol.backend[i]));
                rcprotocol._detected_protocol = (enum rcprotocol_t)i;
                for (uint8_t j = 0; j < RCP_NONE; j++) {
                    if (rcprotocol.backend[j]) {
                        rcprotocol_backend_reset_rc_frame_count(rcprotocol.backend[j]);
                    }
                }
                rcprotocol._last_input_ms = now;
                rcprotocol._detected_with_bytes = false;
                break;
            }
        }
    }
}

/*
  process an array of pulses. n must be even
 */
void rcprotocol_process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap)
{
    if (n & 1) {
        return;
    }
    while (n) {
        uint32_t widths0 = widths[0];
        uint32_t widths1 = widths[1];
        if (need_swap) {
            uint32_t tmp = widths1;
            widths1 = widths0;
            widths0 = tmp;
        }
        widths1 -= widths0;
        rcprotocol_process_pulse(widths0, widths1);
        widths += 2;
        n -= 2;
    }
}

bool rcprotocol_process_byte(uint8_t byte, uint32_t baudrate)
{
    uint32_t now = time_millis();
    bool searching = rcprotocol_should_search(now);

    rcprotocol.rc_protocols_mask = rcs_enabled_protocols();

    if (rcprotocol._detected_protocol != RCP_NONE &&
        !rcprotocol_protocol_enabled(rcprotocol._detected_protocol)) {
        rcprotocol._detected_protocol = RCP_NONE;
    }

    if (rcprotocol._detected_protocol != RCP_NONE && !rcprotocol._detected_with_bytes && !searching) {
        // we're using pulse inputs, discard bytes
        return false;
    }

    // first try current protocol
    if (rcprotocol._detected_protocol != RCP_NONE && !searching) {
        rcprotocol_backend_process_byte(rcprotocol.backend[rcprotocol._detected_protocol], byte, baudrate);
        if (rcprotocol_backend_new_input(rcprotocol.backend[rcprotocol._detected_protocol] )) {
            rcprotocol._new_input = true;
            rcprotocol._last_input_ms = now;
        }
        return true;
    }

    // otherwise scan all protocols
    for (uint8_t i = 0; i < RCP_NONE; i++) {
        if (rcprotocol.backend[i] != NULL) {
            if (!rcprotocol_protocol_enabled((enum rcprotocol_t)i)) {
                continue;
            }
            const uint32_t frame_count = rcprotocol_backend_get_rc_frame_count(rcprotocol.backend[i]);
            const uint32_t input_count = rcprotocol_backend_get_rc_input_count(rcprotocol.backend[i]);
            rcprotocol_backend_process_byte(rcprotocol.backend[i], byte, baudrate);
            const uint32_t frame_count2 = rcprotocol_backend_get_rc_frame_count(rcprotocol.backend[i]);
            if (frame_count2 > frame_count) {
                if (rcprotocol_requires_3_frames((enum rcprotocol_t)i) && frame_count2 < 3) {
                    continue;
                }
                rcprotocol._new_input = (input_count != rcprotocol_backend_get_rc_input_count(rcprotocol.backend[i]));
                rcprotocol._detected_protocol = (enum rcprotocol_t)i;
                rcprotocol._last_input_ms = now;
                rcprotocol._detected_with_bytes = true;
                for (uint8_t j = 0; j < RCP_NONE; j++) {
                    if (rcprotocol.backend[j]) {
                        rcprotocol_backend_reset_rc_frame_count(rcprotocol.backend[j]);
                    }
                }
                // stop decoding pulses to save CPU
                rc_hal_pulse_input_enable(false);
                break;
            }
        }
    }
    return false;
}

// handshake if nothing else has succeeded so far
void rcprotocol_process_handshake(uint32_t baudrate)
{
    // if we ever succeeded before then do not handshake
    if (rcprotocol._detected_protocol != RCP_NONE || rcprotocol._last_input_ms > 0) {
        return;
    }

    // otherwise handshake all protocols
    for (uint8_t i = 0; i < RCP_NONE; i++) {
        if (rcprotocol.backend[i] != NULL) {
            rcprotocol_backend_process_handshake(rcprotocol.backend[i], baudrate);
        }
    }
}

static void rcprotocol_check_added_uart(void)
{
    if (!rcprotocol.added.uart) {
        return;
    }
    uint32_t now = time_millis();
    bool searching = rcprotocol_should_search(now);
    if (!searching && !rcprotocol._detected_with_bytes) {
        // not using this uart
        return;
    }
    if (!rcprotocol.added.opened) {
        rcprotocol.added.opened = true;
        rcprotocol.added.last_config_change_ms = time_millis();
        serialconfig_apply_to_uart(&serial_configs[rcprotocol.added.config_num], rcprotocol.added.uart);
    }

    rcprotocol.rc_protocols_mask = rcs_enabled_protocols();

    const uint32_t current_baud = serial_configs[rcprotocol.added.config_num].baud;
    rcprotocol_process_handshake(current_baud);

    uint32_t n = SerialManager_rx_available(rcprotocol.added.uart);
    n = MIN(n, 255U);
    for (uint8_t i=0; i<n; i++) {
        int16_t b = SerialManager_read_byte(rcprotocol.added.uart);
        if (b >= 0) {
            rcprotocol_process_byte((uint8_t)(b), current_baud);
        }
    }
    if (searching) {
        if (now - rcprotocol.added.last_config_change_ms > 1000) {
            // change configs if not detected once a second
            rcprotocol.added.config_num++;
            if (rcprotocol.added.config_num >= ARRAY_SIZE(serial_configs)) {
                rcprotocol.added.config_num = 0;
            }
            rcprotocol.added.opened = false;
        }
    }
}

void rcprotocol_update()
{
    rcprotocol_check_added_uart();
}

bool rcprotocol_new_input()
{
    bool ret = rcprotocol._new_input;
    rcprotocol._new_input = false;

    // if we have an extra UART from a SERIALn_PROTOCOL then check it for data
    //check_added_uart();

    // run update function on backends
    for (uint8_t i = 0; i < RCP_NONE; i++) {
        if (rcprotocol.backend[i] != NULL) {
            rcprotocol_backend_update(rcprotocol.backend[i]);
        }
    }
    return ret;
}

uint8_t rcprotocol_num_channels()
{
    if (rcprotocol._detected_protocol != RCP_NONE) {
        return rcprotocol_backend_num_channels(rcprotocol.backend[rcprotocol._detected_protocol]);
    }
    return 0;
}

uint16_t rcprotocol_read(uint8_t chan)
{
    if (rcprotocol._detected_protocol != RCP_NONE) {
        return rcprotocol_backend_read(rcprotocol.backend[rcprotocol._detected_protocol], chan);
    }
    return 0;
}

void rcprotocol_read2(uint16_t *pwm, uint8_t n)
{
    if (rcprotocol._detected_protocol != RCP_NONE) {
        rcprotocol_backend_read2(rcprotocol.backend[rcprotocol._detected_protocol], pwm, n);
    }
}

int16_t rcprotocol_get_RSSI(void)
{
    if (rcprotocol._detected_protocol != RCP_NONE) {
        return rcprotocol_backend_get_RSSI(rcprotocol.backend[rcprotocol._detected_protocol]);
    }
    return -1;
}

int16_t rcprotocol_get_rx_link_quality(void)
{
    if (rcprotocol._detected_protocol != RCP_NONE) {
        return rcprotocol_backend_get_rx_link_quality(rcprotocol.backend[rcprotocol._detected_protocol]);
    }
    return -1;
}
/*
  ask for bind start on supported receivers (eg spektrum satellite)
 */
void rcprotocol_start_bind(void)
{
    for (uint8_t i = 0; i < RCP_NONE; i++) {
        if (rcprotocol.backend[i] != NULL) {
            rcprotocol_backend_start_bind(rcprotocol.backend[i]);
        }
    }
}

/*
  return protocol name
 */
const char *rcprotocol_protocol_name_from_protocol(enum rcprotocol_t protocol)
{
    switch (protocol) {
    case RCP_PPM:
        return "PPM";
    case RCP_IBUS:
        return "IBUS";
    case RCP_SBUS:
    case RCP_SBUS_NI:
        return "SBUS";
#if AP_RCPROTOCOL_FASTSBUS_ENABLED
    case RCP_FASTSBUS:
        return "FastSBUS";
#endif
    case RCP_DSM:
        return "DSM";
    case RCP_SUMD:
        return "SUMD";
    case RCP_SRXL:
        return "SRXL";
    case RCP_SRXL2:
        return "SRXL2";
    case RCP_CRSF:
        return "CRSF";
    case RCP_ST24:
        return "ST24";
    case RCP_FPORT:
        return "FPORT";
    case RCP_FPORT2:
        return "FPORT2";
    case RCP_NONE:
        break;
    }
    return NULL;
}

/*
  return protocol name
 */
const char *rcprotocol_protocol_name(void)
{
    return rcprotocol_protocol_name_from_protocol(rcprotocol._detected_protocol);
}

bool rcprotocol_failsafe_active() {
    return rcprotocol._failsafe_active;
}
void rcprotocol_set_failsafe_active(bool active) {
    rcprotocol._failsafe_active = active;
}

void rcprotocol_disable_for_pulses(enum rcprotocol_t protocol) {
    rcprotocol._disabled_for_pulses |= (1U<<(uint8_t)protocol);
}

// return true if we are decoding a byte stream, instead of pulses
bool rcprotocol_using_uart(void) {
    return rcprotocol._detected_with_bytes;
}


// return true if a specific protocol is enabled
static bool rcprotocol_protocol_enabled(enum rcprotocol_t protocol)
{
    if ((rcprotocol.rc_protocols_mask & 1) != 0) {
        // all protocols enabled
        return true;
    }
    return ((1U<<((uint8_t)(protocol)+1)) & rcprotocol.rc_protocols_mask) != 0;
}

/*
  check for bytes from an additional uart. This is used to support RC
  protocols from SERIALn_PROTOCOL
 */
static void serialconfig_apply_to_uart(const struct SerialConfig* serialconfig, rt_device_t uart)
{
#if 0
    uart->configure_parity(parity);
    uart->set_stop_bits(stop_bits);
    if (invert_rx) {
        uart->set_options(uart->get_options() | AP_HAL::UARTDriver::OPTION_RXINV);
    } else {
        uart->set_options(uart->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV);
    }
    uart->begin(baud, 128, 128);
#endif
}

/*------------------------------------test------------------------------------*/


